/***********************************************************************/
/* Name:       Xuqing Wu                                               */
/* Email:      xwu@cs.uh.edu                                           */
/* QIL Lab, CS, U. of Houston                                          */
/*                                                                     */
/* All Rights Reserved                                                  */
/* THIS SOURCE CODE IS PROVIDED 'AS IS', WITH NO WARRANTIES WHATSOEVER */
/***********************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "uthash.h"

#ifndef USEMATLAB
#include <unistd.h>
#endif
#ifdef USEMATLAB
#include "mex.h"
#include "matrix.h"
#endif

#define U8  unsigned char
#define U32 unsigned int

#define BUF_SIZE        128
#define EPSILON         0.00000001
#define BOXID1          0
#define BOXID2          1
#define NOTSET          -0xFF

#ifdef USEMATLABMEM
#define malloc mxMalloc
#define free mxFree
#endif

#ifndef NUMOFSTATE
#define NUMOFSTATE      2
#endif
#ifndef MAXDATADIM
#define MAXDATADIM      1
#endif

#if defined(NEIGHBOR_4)
#define NE  0.5
#elif defined(NEIGHBOR_8)
#define NE 0.25
#else
#define NE 0.5
#endif

/*
#define PDEB(a, b, fmt, args...)          \
do                                        \
{                                         \
   if (a >= b)                            \
   {                                      \
      printf(fmt, ## args);               \
   }                                      \
} while(0)                                \
*/

typedef struct coupblief
{
   double val;
   struct node *pnbr;
} coupblief_t;

/* structure for messages in message box
 * val: value of message
 * */
typedef struct message
{
   int      idx;
   double   val;
   struct node *source;
} message_t;

/* structure for each node 
 * two message boxes: one for new message, one for old message
 * nbr: number of neighbors
 * bi: blief value for current node
 * bij: bleif value for pair nodes
 * clocal: pointer to local energy function
 * cpair: pointer to pair energy function
 * next: pointer to neighbors
 * nhash: hash table for neighboring nodes
 */
typedef struct node
{
   int      idx;
   int      y[NUMOFSTATE];
   int      ymax;
   double   *val;
   double   **data;
   int      nbrs;

   double      bi[NUMOFSTATE];
   double      (*clocal)(struct node *pnode, int yi, double *para);
   double      (*cpair)(struct node *inode, struct node *jnode, int yi, 
                        int yj, double *para);
   struct message    *mbox1[NUMOFSTATE];
   struct message    *mbox2[NUMOFSTATE];
   struct coupblief  *bij[NUMOFSTATE*NUMOFSTATE];
   struct nodehash   *nhash;
} node_t;

typedef struct nodehash
{
   struct node *key;
   int    nid;
   UT_hash_handle hh;
} nodehash_t;

/* add neighbor to a node
 * the new neighbor is alway added to the end of nodelist
 */
void setneighbor(node_t *pnode, node_t *pnbr)
{
   nodehash_t *nhash;
   
   if(pnode == NULL || pnbr == NULL || pnode == pnbr) {
      printf("ERROR: NULL or invalid pointer!\n");
      return;
   }
   //printf("Start adding neighbors...");
   
   /* set hash for the neighbor 
    */
   nhash = (nodehash_t *)malloc(sizeof(nodehash_t));
   nhash->key = pnbr;
   nhash->nid = pnode->nbrs;
   HASH_ADD_PTR(pnode->nhash, key, nhash);

   pnode->nbrs++;
   //printf("Finish adding neighbors\n");
}

/* initialize for each node:
 * input:   pointer to the node
 *          node index
 *          node observation value (a pointer to a vector)
 *          pointer to a predefined Y vector (class label)
 * NOTE: bij[i][j] the j is refering to the node in the nodelist
 *       message box is assigned to a neighbor and indexed by its addr
 */
void initnode(node_t *pnode, int idx, double *val, int *pys)
{
   int i;
   node_t *pnbr;
   nodehash_t *phash, *tmphash;

   //printf("Start init node...");

   pnode->idx = idx;
   pnode->val = val;
   pnode->ymax = -1;
   for(i=0;i<NUMOFSTATE;i++) {
      pnode->y[i] = pys[i];
   }

   for(i=0;i<NUMOFSTATE;i++) {
      pnode->mbox1[i] = (message_t *) malloc(sizeof(message_t)*pnode->nbrs);
      memset((char *)pnode->mbox1[i], 0, sizeof(message_t)*pnode->nbrs);
   }
   for(i=0;i<NUMOFSTATE;i++) {
      pnode->mbox2[i] = (message_t *) malloc(sizeof(message_t)*pnode->nbrs);
      memset((char *)pnode->mbox2[i], 0, sizeof(message_t)*(pnode->nbrs));
   }
   
   memset((char *)pnode->bi, 0, sizeof(double)*NUMOFSTATE);
   for(i=0;i<NUMOFSTATE*NUMOFSTATE;i++) {
      pnode->bij[i] = (coupblief_t *) malloc(sizeof(coupblief_t)*pnode->nbrs);
      memset((char *)pnode->bij[i], 0, sizeof(coupblief_t)*pnode->nbrs);
   }

   for(tmphash=pnode->nhash; tmphash!=NULL; tmphash=tmphash->hh.next) {
      pnbr = tmphash->key;
      for(i=0;i<NUMOFSTATE;i++) {
         HASH_FIND_PTR(pnode->nhash, &pnbr, phash);
         pnode->mbox1[i][phash->nid].source = pnbr;
         pnode->mbox2[i][phash->nid].source = pnbr;
         pnode->mbox1[i][phash->nid].val = 1.0;
         pnode->mbox2[i][phash->nid].val = 1.0;
      }
   }

   //printf("Finish init node\n");
}

